#include "servo.h"

int servo_mode = 0;

void servo_setDutyCycleA( int dutyCycle )
{
   if(servo_mode < 3)
   {
       OCR1A = dutyCycle; 
   }
}

void servo_setDutyCycleB( int dutyCycle )
{
   if(servo_mode < 3)
   {
      OCR1B = dutyCycle; 
   }
}

void servo_setDutyCycleC( int dutyCycle )
{
   if(servo_mode < 3)
   {
      OCR1C = dutyCycle; 
   }
}
/*
 * This servo driver will be able to use the 16 bit and both 8 bit counters to run PWM
 * This drive is specifically for the HTec HS-55 servo
 *
 * For using the 16 bit TCNT, use the following modes 
 * Mode 0 is default mode in which only Output Compare Register A is used to run a PWM
 * Mode 1 sets A,B OC's to run a PWM.
 * Mode 2 sets all A,B,C OC's to run a PWM.
 */
void init_servo(int m)
{
   servo_mode = m;
   /* Set Clock select to 010 or 2 for a division by 8
      Set Wave Generator WGM to 14 or 1110, you can only do 
	  the two most siginifant bits in the TCCR1B register. A WGM of 14
	  uses ICR1 as TOP in Fast PWM mode;
   */
   TCCR1B = (0<<CS10)|(1<<CS11)|(0<<CS12)|(1<<WGM13)|(1<<WGM12);  
   /* Set WGM two LSB's to get a WGM of 14 and set the logical changes to the OCA1, OCB1 and 
      OCC1 pins by setting their respective COM value to 2: which is set output to 0 on 
	  successful compare of TCNT1 and OCR1A/B/C and set ouput to 1 when TCNT1 = ICR1(TOP)*/
   if(servo_mode == 0){
       TCCR1A = (0<<WGM10) | (1<<WGM11) | (1<<COM1A1) | (0<<COM1A0); 
       /*Set the IO pins for output for OCA1, OCB1, OCC1*/ 
       DDRA = 0xFF;
	   /* Set the TOP value to 2500 this should mean that the TCNT1 counter
       should reset itself every 50 Hz at a clock of 1Mz for 8 Mhz, set to 20000(hopefully)*/
	   ICR1 = 20000;   
	   /* Set the low timings for the three output pins for ports A in OCR1A*/
	   OCR1A = 1600; //corresponds to 8% duty cycle or 2ms pulse width
   }else if(servo_mode == 1){
       TCCR1A = (0<<WGM10) | (1<<WGM11) | (1<<COM1A1) | (0<<COM1A0) | (1<<COM1B1) | (0<<COM1B0);
       /*Set the IO pins for output for OCA1, OCB1, OCC1*/ 
       DDRA = 0xFF;
       DDRB = 0xFF;     
	   /* Set the TOP value to 2500 this should mean that the TCNT1 counter
	      should reset itself every 50 Hz at a clock of 1Mz for 8 Mhz, set to 20000(hopefully)*/
	   ICR1 = 20000;   
	   /* Set the low timings for the three output pins for ports A,B in OCR1A/B */
	   OCR1A = 1600; //corresponds to 8% duty cycle or 2ms pulse width
	   OCR1B = 1100; //corresponds to 5.5% duty cycle or 
   }else if(servo_mode == 2){
       TCCR1A = (0<<WGM10) | (1<<WGM11) | (1<<COM1A1) | (0<<COM1A0) | (1<<COM1B1) | (0<<COM1B0) | (1<<COM1C1) | (0<<COM1C0);
       /*Set the IO pins for output for OCA1, OCB1, OCC1*/
	   DDRA = 0xFF;
   	   DDRB = 0xFF;
       DDRC = 0xFF;
	   /* Set the TOP value to 2500 this should mean that the TCNT1 counter
       should reset itself every 50 Hz at a clock of 1Mz for 8 Mhz, set to 20000(hopefully)*/
	   ICR1 = 20000;   
	   /* Set the low timings for the three output pins for ports A,B,C in OCR1A/B/C*/
	   OCR1A = 1600; //corresponds to 8% duty cycle or 2ms pulse width
	   OCR1B = 1100; //corresponds to 5.5% duty cycle or 
	   OCR1C = 600; //corresponds to 3% duty cycle 900 us pulse width
   }
}


